#include "line_alignment/line_aligner.hpp"
#include <jsoncpp/json/json.h>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <rclcpp_lifecycle/lifecycle_publisher.hpp>
#include <lifecycle_msgs/msg/transition.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp> 
#include <std_msgs/msg/string.hpp> 
#include <std_msgs/msg/bool.hpp>    
#include <rclcpp/time.hpp>      
#include <chrono>               
#include <limits>
#include <algorithm>
#include <vector>
#include <tuple>
#include <numeric>
#include <cmath>
#include <mutex>
#include <atomic>
#include <functional>
#include <memory>          
#include <string>



namespace line_alignment {

    LineAligner::LineAligner(const rclcpp::NodeOptions & options)
        : rclcpp_lifecycle::LifecycleNode("line_aligner", options)
    { 
        RCLCPP_INFO(this->get_logger(), "Lifecycle Line Aligner is now created.");
        // 不再在此处初始化，因为生命周期节点的初始化由状态机驱动
    }


    // 声明参数
    void LineAligner::declare_parameters()
    { 
        params_.pointcloud_topic = this->declare_parameter("pointcloud_topic", "mqtt_reconstructed_cloud");
        params_.cmd_vel_topic = this->declare_parameter("cmd_vel_topic", "cmd_vel");
        params_.completed_topic = this->declare_parameter("completed_topic", "completed");
        params_.controlling_period = this->declare_parameter("controlling_period", 125);
        params_.n_x_limit_max = this->declare_parameter("n_x_limit_max", 0.2);
        params_.n_x_limit_min = this->declare_parameter("n_x_limit_min", -0.2);
        params_.n_y_limit_max = this->declare_parameter("n_y_limit_max", 0.2);
        params_.n_y_limit_min = this->declare_parameter("n_y_limit_min", -0.2);
        params_.g_x_limit_max = this->declare_parameter("g_x_limit_max", 0.09);
        params_.g_x_limit_min = this->declare_parameter("g_x_limit_min", -0.09);
        params_.g_y_limit_max = this->declare_parameter("g_y_limit_max", 0.16);
        params_.g_y_limit_min = this->declare_parameter("g_y_limit_min", -0.16);
        params_.source_type = this->declare_parameter("source_type", "neutron");
        params_.max_velocity = this->declare_parameter("max_velocity", 0.05);
        params_.reaching_tolerance = this->declare_parameter("reaching_tolerance", 0.005);
        params_.sampling_duration_sec = this->declare_parameter("sampling_duration", 5.0);
        sampling_duration_ = std::chrono::duration<double>(params_.sampling_duration_sec);

        params_.SIGMA_THRESHOLD_BOUND_ = this->declare_parameter("sigma_threshold", 0.0);
    }

    CallbackReturn LineAligner::on_configure(const rclcpp_lifecycle::State & state)
    {
        RCLCPP_INFO(this->get_logger(), "Configuring Line Aligner");
        // 声明参数
        declare_parameters();

        // 初始化成员变量
        pointcloud_received_ = false;
        control_state_ = IDLE;
        cloud_x_max = std::numeric_limits<double>::max();;
        cloud_x_min = std::numeric_limits<double>::lowest();
        cloud_y_max = std::numeric_limits<double>::max();;
        cloud_y_min = std::numeric_limits<double>::lowest();
        x_limit_max = 0.0;
        x_limit_min = 0.0;
        y_limit_max = 0.0;
        y_limit_min = 0.0;
        sampling_start_time_ = rclcpp::Time(0, 0, this->get_clock()->get_clock_type());
        x_avg_max = 0.0;
        x_avg_min = 0.0;
        y_avg_max = 0.0;
        y_avg_min = 0.0;
        linear_x = 0.0;
        linear_y = 0.0;
        angular_z = 0.0;

        state_is_active_ = false;

        RCLCPP_INFO(this->get_logger(), "Successfully configured.");
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    }

    CallbackReturn LineAligner::on_activate(const rclcpp_lifecycle::State & state)
    { 
        RCLCPP_INFO(this->get_logger(), "Activating...");

        // 创建发布者和订阅者
        pointcloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
            params_.pointcloud_topic, 10,
            std::bind(&LineAligner::pointcloudCallback, this, std::placeholders::_1));

        cmd_vel_pub_ = this->create_publisher<std_msgs::msg::String>(params_.cmd_vel_topic, 10);
        completed_pub_ = this->create_publisher<std_msgs::msg::Bool>(
            params_.completed_topic, rclcpp::QoS(1).transient_local());

        // 创建控制循环定时器
        control_timer_ = this->create_wall_timer(
            std::chrono::milliseconds(params_.controlling_period),
            std::bind(&LineAligner::controlLoop, this));

        // 激活发布者 (LifecyclePublisher 需要显式激活)
        cmd_vel_pub_->on_activate();
        completed_pub_->on_activate();

        // 重置状态为 IDLE
        control_state_ = IDLE;
        state_is_active_ = true; // 标记为活动状态

        RCLCPP_INFO(this->get_logger(), "Successfully activated.");
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    }


    CallbackReturn LineAligner::on_deactivate(const rclcpp_lifecycle::State & state)
    { 
        RCLCPP_INFO(this->get_logger(), "Deactivating...");

        // 取消定时器
        if(control_timer_){
            control_timer_->cancel();
        }

        // 停用发布者
        if(cmd_vel_pub_) cmd_vel_pub_->on_deactivate();
        if(completed_pub_) completed_pub_->on_deactivate();

        // 停止机器人
        stopRobot();

        state_is_active_ = false;

        RCLCPP_INFO(this->get_logger(), "Successfully deactivated.");
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    }

    CallbackReturn LineAligner::on_cleanup(const rclcpp_lifecycle::State & state)
    { 
        RCLCPP_INFO(this->get_logger(), "Cleaning up...");

        // 重置状态标志
        state_is_active_ = false;

        // 重置所有内部状态变量
        pointcloud_received_ = false;
        control_state_ = IDLE;
        cloud_x_max = std::numeric_limits<double>::max();;
        cloud_x_min = std::numeric_limits<double>::lowest();
        cloud_y_max = std::numeric_limits<double>::max();;
        cloud_y_min = std::numeric_limits<double>::lowest();
        x_limit_max = 0.0;
        x_limit_min = 0.0;
        y_limit_max = 0.0;
        y_limit_min = 0.0;
        sampling_start_time_ = rclcpp::Time(0, 0, this->get_clock()->get_clock_type());
        x_avg_max = 0.0;
        x_avg_min = 0.0;
        y_avg_max = 0.0;
        y_avg_min = 0.0;
        linear_x = 0.0;
        linear_y = 0.0;
        angular_z = 0.0;

        sample_range_.clear(); // 清空样本集

        // 重置ROS接口指针
        pointcloud_sub_.reset();
        cmd_vel_pub_.reset();
        completed_pub_.reset();

        RCLCPP_INFO(this->get_logger(), "Successfully cleaned up.");
        return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
    }


    void LineAligner::pointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
    { 
        // 如果节点未激活，则不处理数据
        if (!state_is_active_) {
            return;
        }

        // 转换为PCL点云
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
        pcl::fromROSMsg(*msg, *cloud);
        RCLCPP_INFO(this->get_logger(), "Received point cloud with %zu points", cloud->size());

        if(cloud->empty()) {
            RCLCPP_WARN(this->get_logger(), "Point cloud is empty.");
            return;
        }

        // 临时变量用于计算当前点云的边界
        double temp_x_min = std::numeric_limits<double>::max();
        double temp_x_max = std::numeric_limits<double>::lowest();
        double temp_y_min = std::numeric_limits<double>::max();
        double temp_y_max = std::numeric_limits<double>::lowest();

        // 遍历所有点，计算XY边界值
        for (const auto& point : cloud->points) {
            // 跳过无效点
            if (!std::isfinite(point.x) || !std::isfinite(point.y) || !std::isfinite(point.z)) {
                continue;
            }
            
            // 更新临时变量
            temp_x_min = std::min(temp_x_min, static_cast<double>(point.x));
            temp_x_max = std::max(temp_x_max, static_cast<double>(point.x));
            temp_y_min = std::min(temp_y_min, static_cast<double>(point.y));
            temp_y_max = std::max(temp_y_max, static_cast<double>(point.y));
        }

        // 在临界区外完成计算，只在最后更新共享变量
        {
            std::lock_guard<std::mutex> lock(cloud_bounds_mutex_);
            cloud_x_min = temp_x_min;
            cloud_x_max = temp_x_max;
            cloud_y_min = temp_y_min;
            cloud_y_max = temp_y_max;
            pointcloud_received_ = true; 
        }

        RCLCPP_INFO(this->get_logger(), "X:[%.3f, %.3f], Y:[%.3f, %.3f]", cloud_x_min, cloud_x_max, cloud_y_min, cloud_y_max);
    }


    void LineAligner::controlLoop()
    { 
        // 如果节点未激活，则不处理数据
        if (!state_is_active_) {
            return;
        }

        switch (control_state_)
        { 
            case IDLE: 
                handleIdleState();
                break;
            case MOVE: 
                handleMoveState();
                break;
            case SAMPLING: 
                handleSamplingState();
                break;
            case SIDEMOVE: 
                handleSideMoveState();
                break;
            case VERIFYING: 
                handleVerifyingState();
                break;
            case COMPLETED: 
                handleCompletedState();
                break;
        }
    }

    void LineAligner::handleIdleState()
    { 
        if (!pointcloud_received_) {
            stopRobot();
            return;
        }

        // 根据放射源不同设置点云极限值
        if (params_.source_type == "neutron") {
            x_limit_min = params_.n_x_limit_min;
            x_limit_max = params_.n_x_limit_max;
            y_limit_min = params_.n_y_limit_min;
            y_limit_max = params_.n_y_limit_max;
        } else if (params_.source_type == "gamma") {
            x_limit_min = params_.g_x_limit_min;
            x_limit_max = params_.g_x_limit_max;
            y_limit_min = params_.g_y_limit_min;
            y_limit_max = params_.g_y_limit_max;
        }

        control_state_ = MOVE;
        RCLCPP_INFO(this->get_logger(), "[IDLE] 正切换到 MOVE state...");
    }

    void LineAligner::handleMoveState()
    { 
        // 在临界区内读取边界值，确保一致性
        double current_cloud_x_min, current_cloud_x_max, current_cloud_y_min, current_cloud_y_max;
        bool current_pointcloud_received;
        {
            std::lock_guard<std::mutex> lock(cloud_bounds_mutex_);
            current_cloud_x_min = cloud_x_min;
            current_cloud_x_max = cloud_x_max;
            current_cloud_y_min = cloud_y_min;
            current_cloud_y_max = cloud_y_max;
            current_pointcloud_received = pointcloud_received_;
        }

        if (!current_pointcloud_received) {
            stopRobot();
            control_state_ = IDLE;
            return;
        }

        // 将点云x轴边界与极限值对齐
        if(current_cloud_x_min <= x_limit_min - params_.reaching_tolerance) {
            stopRobot();
            control_state_ = SAMPLING;
            RCLCPP_INFO(this->get_logger(), "[Move]X轴已对齐, 现在切换到 SAMPLING state...");
        }else {
            // 若点云x轴负轴边界大于限制值，则输出正向速度值
            double distance = current_cloud_x_min - x_limit_min;
            linear_x = params_.max_velocity * (distance > 0 ? 1 : -1);
            linear_y = 0.0;
            angular_z = 0.0;
            publishVelocity();
            RCLCPP_INFO(this->get_logger(),"[Move]速度为: lineax: %.3f, lineary: %.3f, angularz: %.3f !",
                        linear_x, linear_y, angular_z);
        }
    }

    void LineAligner::handleSamplingState()
    { 
        if (!pointcloud_received_) {
            stopRobot();
            control_state_ = IDLE;
            return;
        }

        // 记录第一个样本的时间
        if (sample_range_.empty()) {
            sampling_start_time_ = this->now();
        }

        // 添加当前数据到样本
        sample_range_.push_back(SampleRange(cloud_x_min, cloud_x_max, cloud_y_min, cloud_y_max));
        
        //检查是否收集足够时间的样本 
        auto current_time = this->now();
        auto elapsed_time = (current_time - sampling_start_time_).seconds();

        if(elapsed_time >= sampling_duration_.count()) {
            processSamples();
            sample_range_.clear();

            // 根据y轴点云边界值与极限值对比判断控制方式
            if((y_avg_max >= y_limit_max - params_.reaching_tolerance) &&
                (y_avg_max <= y_limit_max + params_.reaching_tolerance)){
                stopRobot();
                control_state_ = VERIFYING;
                RCLCPP_INFO(this->get_logger(), "[Move]Y轴已对齐, 现在切换到 VERIFYING state...");
            } else {
                stopRobot();
                control_state_ = SIDEMOVE;
                RCLCPP_INFO(this->get_logger(), "[Move]Y轴未对齐, 现在切换到 SIDEMOVE state...");
            }
        }
    }


    void LineAligner::handleSideMoveState()
    {
        // 在临界区内读取边界值，确保一致性
        double current_cloud_y_max, current_cloud_y_min; // Only read Y bounds for side move
        bool current_pointcloud_received;
        {
            std::lock_guard<std::mutex> lock(cloud_bounds_mutex_);
            current_cloud_y_max = cloud_y_max;
            current_cloud_y_min = cloud_y_min; // Might need this for logic too
            current_pointcloud_received = pointcloud_received_;
        }

        if (!current_pointcloud_received) {
            stopRobot();
            control_state_ = IDLE;
            return;
        }

        if(std::abs(current_cloud_y_max - y_limit_max) <= params_.reaching_tolerance){
            stopRobot();
            control_state_ = VERIFYING;
            RCLCPP_INFO(this->get_logger(), "[SideMove]Y轴已对齐, 现在切换到 VERIFYING state...");
        }
        else {
            double distance = current_cloud_y_max - y_limit_max;
            linear_x = 0.0;
            linear_y = params_.max_velocity * (distance > 0 ? 1 : -1);
            angular_z = 0.0;
            publishVelocity();
            RCLCPP_INFO(this->get_logger(),"[SideMove]速度为: lineax: %.3f, lineary: %.3f, angularz: %.3f !",
                        linear_x, linear_y, angular_z);
        }
    }

    void LineAligner::handleVerifyingState()
    { 
        if (!pointcloud_received_) {
            stopRobot();
            control_state_ = IDLE;
            return;
        }

        // 记录第一个样本的时间
        if (sample_range_.empty()) {
            sampling_start_time_ = this->now();
        }

        // 添加当前数据到样本
        sample_range_.push_back(SampleRange(cloud_x_min, cloud_x_max, cloud_y_min, cloud_y_max));
        
        //检查是否收集足够时间的样本 
        auto current_time = this->now();
        auto elapsed_time = (current_time - sampling_start_time_).seconds();

        if(elapsed_time >= sampling_duration_.count()) {
            processSamples();
            sample_range_.clear();

            bool x_ok = ((x_avg_min >= x_limit_min - params_.reaching_tolerance) &&
                        (x_avg_min <= x_limit_min + params_.reaching_tolerance));
            bool y_ok = ((y_avg_max >= y_limit_max - params_.reaching_tolerance) &&
                        (y_avg_max <= y_limit_max + params_.reaching_tolerance));
            if(x_ok && y_ok){
                //完全匹配
                stopRobot();
                control_state_ = COMPLETED;
                RCLCPP_INFO(this->get_logger(), "[Verifying]完全匹配, 现在切换到 COMPLETED state...");
            } else {
                if(!x_ok){
                    // 退回Move
                    control_state_ = MOVE;
                    RCLCPP_INFO(this->get_logger(), "[Verifying]X轴未对齐, 现在切换到 MOVE state...");
                } else {
                    control_state_ = SIDEMOVE;
                    RCLCPP_INFO(this->get_logger(), "[Verifying]Y轴未对齐, 现在切换到 SIDEMOVE state...");
                }
            }
        }
    }

    void LineAligner::handleCompletedState()
    { 
        stopRobot();
        publishCompleted(true);
    }


    void LineAligner::processSamples()
    {
        if (sample_range_.size() < 3) {
            // 样本太少，使用当前值 (假设 cloud_x_min, cloud_x_max, cloud_y_min, cloud_y_max 是当前的边界)
            double current_x_min, current_x_max, current_y_min, current_y_max;
            {
                std::lock_guard<std::mutex> lock(cloud_bounds_mutex_);
                current_x_min = cloud_x_min;
                current_x_max = cloud_x_max;
                current_y_min = cloud_y_min;
                current_y_max = cloud_y_max;
            }
            x_avg_max = current_x_max;
            x_avg_min = current_x_min;
            y_avg_max = current_y_max;
            y_avg_min = current_y_min;
            return;
        }

        // 去除离群值（使用3σ原则）
        // 提取 x_min, x_max, y_min, y_max 值到独立的向量
        std::vector<double> x_min_vals, x_max_vals, y_min_vals, y_max_vals;
        x_min_vals.reserve(sample_range_.size());
        x_max_vals.reserve(sample_range_.size());
        y_min_vals.reserve(sample_range_.size());
        y_max_vals.reserve(sample_range_.size());

        // 遍历 sample_range_，使用成员函数获取值
        for (const auto& bounds_struct : sample_range_) {
            x_min_vals.push_back(bounds_struct.getXMin());
            x_max_vals.push_back(bounds_struct.getXMax());
            y_min_vals.push_back(bounds_struct.getYMin());
            y_max_vals.push_back(bounds_struct.getYMax());
        }

        // 计算均值和标准差
        auto [filtered_x_min, filtered_x_max, filtered_y_min, filtered_y_max] = removeOutliersBounds(
            x_min_vals, x_max_vals, y_min_vals, y_max_vals);

        // 计算平均值
        x_avg_min = std::accumulate(filtered_x_min.begin(), filtered_x_min.end(), 0.0) / filtered_x_min.size();
        x_avg_max = std::accumulate(filtered_x_max.begin(), filtered_x_max.end(), 0.0) / filtered_x_max.size();
        y_avg_min = std::accumulate(filtered_y_min.begin(), filtered_y_min.end(), 0.0) / filtered_y_min.size();
        y_avg_max = std::accumulate(filtered_y_max.begin(), filtered_y_max.end(), 0.0) / filtered_y_max.size();

        RCLCPP_INFO(this->get_logger(),
                    "Processed samples: x_avg_min=%.3f (from %zu->%zu), x_avg_max=%.3f (from %zu->%zu), y_avg_min=%.3f (from %zu->%zu), y_avg_max=%.3f (from %zu->%zu)",
                    x_avg_min, x_min_vals.size(), filtered_x_min.size(),
                    x_avg_max, x_max_vals.size(), filtered_x_max.size(),
                    y_avg_min, y_min_vals.size(), filtered_y_min.size(),
                    y_avg_max, y_max_vals.size(), filtered_y_max.size());
    }


    // 为边界值实现离群值去除函数
    std::tuple<std::vector<double>, std::vector<double>, std::vector<double>, std::vector<double>>
    LineAligner::removeOutliersBounds(
        const std::vector<double>& x_min_coords,
        const std::vector<double>& x_max_coords,
        const std::vector<double>& y_min_coords,
        const std::vector<double>& y_max_coords)
    {
        // 检查输入大小是否一致
        if (x_min_coords.size() != x_max_coords.size() ||
            x_min_coords.size() != y_min_coords.size() ||
            x_min_coords.size() != y_max_coords.size() ||
            x_min_coords.size() < 3) {
            RCLCPP_WARN(this->get_logger(), "Invalid input size for outlier removal, returning original values.");
            return {x_min_coords, x_max_coords, y_min_coords, y_max_coords};
        }

        // 计算 x_min 坐标均值和标准差
        double x_min_mean = std::accumulate(x_min_coords.begin(), x_min_coords.end(), 0.0) / x_min_coords.size();
        double x_min_var = 0.0;
        for (double x_min : x_min_coords) {
            x_min_var += (x_min - x_min_mean) * (x_min - x_min_mean);
        }
        x_min_var /= x_min_coords.size();
        double x_min_std = std::sqrt(x_min_var);

        // 计算 x_max 坐标均值和标准差
        double x_max_mean = std::accumulate(x_max_coords.begin(), x_max_coords.end(), 0.0) / x_max_coords.size();
        double x_max_var = 0.0;
        for (double x_max : x_max_coords) {
            x_max_var += (x_max - x_max_mean) * (x_max - x_max_mean);
        }
        x_max_var /= x_max_coords.size();
        double x_max_std = std::sqrt(x_max_var);

        // 计算 y_min 坐标均值和标准差
        double y_min_mean = std::accumulate(y_min_coords.begin(), y_min_coords.end(), 0.0) / y_min_coords.size();
        double y_min_var = 0.0;
        for (double y_min : y_min_coords) {
            y_min_var += (y_min - y_min_mean) * (y_min - y_min_mean);
        }
        y_min_var /= y_min_coords.size();
        double y_min_std = std::sqrt(y_min_var);

        // 计算 y_max 坐标均值和标准差
        double y_max_mean = std::accumulate(y_max_coords.begin(), y_max_coords.end(), 0.0) / y_max_coords.size();
        double y_max_var = 0.0;
        for (double y_max : y_max_coords) {
            y_max_var += (y_max - y_max_mean) * (y_max - y_max_mean);
        }
        y_max_var /= y_max_coords.size();
        double y_max_std = std::sqrt(y_max_var);

        // 去除离群值
        std::vector<double> filtered_x_min, filtered_x_max, filtered_y_min, filtered_y_max;
        for (size_t i = 0; i < x_min_coords.size(); ++i) {
            bool is_valid = true;
            // 检查 x_min
            if (x_min_std > 0 && std::abs(x_min_coords[i] - x_min_mean) > params_.SIGMA_THRESHOLD_BOUND_ * x_min_std) {
                is_valid = false;
            }
            // 检查 x_max
            if (is_valid && x_max_std > 0 && std::abs(x_max_coords[i] - x_max_mean) > params_.SIGMA_THRESHOLD_BOUND_ * x_max_std) {
                is_valid = false;
            }
            // 检查 y_min
            if (is_valid && y_min_std > 0 && std::abs(y_min_coords[i] - y_min_mean) > params_.SIGMA_THRESHOLD_BOUND_ * y_min_std) {
                is_valid = false;
            }
            // 检查 y_max
            if (is_valid && y_max_std > 0 && std::abs(y_max_coords[i] - y_max_mean) > params_.SIGMA_THRESHOLD_BOUND_ * y_max_std) {
                is_valid = false;
            }

            if (is_valid) {
                filtered_x_min.push_back(x_min_coords[i]);
                filtered_x_max.push_back(x_max_coords[i]);
                filtered_y_min.push_back(y_min_coords[i]);
                filtered_y_max.push_back(y_max_coords[i]);
            }
        }

        // 如果过滤后样本太少，可能说明阈值太严格或数据本身有异常，可以返回原始值或警告
        if (filtered_x_min.empty()) {
            RCLCPP_WARN(this->get_logger(), "All samples considered outliers after filtering. Returning original values.");
            return {x_min_coords, x_max_coords, y_min_coords, y_max_coords};
        }

        return {filtered_x_min, filtered_x_max, filtered_y_min, filtered_y_max};
    }

    void LineAligner::Reset() {
        linear_x = 0.0;
        linear_y = 0.0;
        angular_z = 0.0;
    }

    void LineAligner::stopRobot() {
        Reset();
        publishVelocity();
    }

    void LineAligner::publishVelocity() {
        vel_cmd.set_from_twist(linear_x, linear_y, angular_z);
        std::string json_string = vel_cmd.get_json_string();

        if(!json_string.empty() && json_string != "null") {
            std_msgs::msg::String msg;
            msg.data = json_string;
            cmd_vel_pub_->publish(msg);
        }
    }

    void LineAligner::publishCompleted(bool success) {
        std_msgs::msg::Bool reached_msg;
        reached_msg.data = success;

        if(success) {
            completed_pub_-> publish(reached_msg);
            RCLCPP_INFO(this->get_logger(), "Published REACHED status: success=%s", 
                       success ? "true" : "false");
        } else {
            RCLCPP_INFO(this->get_logger(), "Published REACHED status: success=%s", 
                       success ? "true" : "false");
        }
    }

}

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);

    // 创建生命周期节点
    auto node = std::make_shared<line_alignment::LineAligner>(rclcpp::NodeOptions());

    // 使用 MultiThreadedExecutor 以便正确处理生命周期
    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node->get_node_base_interface());
    executor.spin();

    rclcpp::shutdown();
    return 0;
}